#include "small.h"
#include <Adafruit_PWMServoDriver.h>
#include <SoftwareSerial.h>

Adafruit_PWMServoDriver pwm=Adafruit_PWMServoDriver();
SoftwareSerial BT(11,10);  //RX/TX

const int numberOfServos = 8; // Number of servos
const int numberOfACE = 9; // Number of action code elements
//int servoCal[] = { 10, 5, 0, -13, -10, 10, -3, 5 }; // Servo calibration data 舵机偏移量校准
int servoCal[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
int servoPos[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // Servo current position
int servoPrevPrg[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // Servo previous prg
int servoPrgPeriod = 20; // 50 ms
int servoIndex[] ={0,1,2,3,4,5,6,7}; // Servo object



// Zero
int servoPrg00step = 1;
const int servoPrg00 [][9] PROGMEM  = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {  135,  45, 135,  45,  45, 135,  45, 135, 1000  }, // zero position
};

// Standby
int servoPrg01step = 2;
const int servoPrg01 [][9] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   90,  90,  90,  90,  90,  90,  90,  90,  200  }, // prep standby
  {  -20,   0,   0,  20,  20,   0,   0, -20,  200  }, // standby
};

// Forward
int servoPrg02step = 11;
const int servoPrg02 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {   20,   0,   0,   0,   0,   0, -45,  20,  100  }, // leg1,4 up; leg4 fw
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,   0,   0, -20, -20,   0,   0,   0,  100  }, // leg2,3 up
  {    0, -45,  45,   0,   0,   0,  45,   0,  100  }, // leg1,4 bk; leg2 fw
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {   20,  45,   0,   0,   0,   0,   0,  20,  100  }, // leg1,4 up; leg1 fw
  {    0,   0, -45,   0,   0,  45,   0,   0,  100  }, // leg2,3 bk
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,   0,   0,   0, -20,   0,   0,   0,  100  }, // leg3 up
  {    0,   0,   0,   0,  20, -45,   0,   0,  100  }, // leg3 fw dn
};

// Backward
int servoPrg03step = 11;
const int servoPrg03 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {   20, -45,   0,   0,   0,   0,   0,  20,  100  }, // leg4,1 up; leg1 fw
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg4,1 dn
  {    0,   0,   0, -20, -20,   0,   0,   0,  100  }, // leg3,2 up
  {    0,  45,   0,   0,   0,  45, -45,   0,  100  }, // leg4,1 bk; leg3 fw
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg3,2 dn
  {   20,   0,   0,   0,   0,   0,  45,  20,  100  }, // leg4,1 up; leg4 fw
  {    0,   0,  45,   0,   0, -45,   0,   0,  100  }, // leg3,1 bk
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg4,1 dn
  {    0,   0,   0, -20,   0,   0,   0,   0,  100  }, // leg2 up
  {    0,   0, -45,  20,   0,   0,   0,   0,  100  }, // leg2 fw dn
};

// Move Left
int servoPrg04step = 11;
const int servoPrg04 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {    0,   0, -45, -20, -20,   0,   0,   0,  100  }, // leg3,2 up; leg2 fw
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg3,2 dn
  {   20,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg1,4 up
  {    0,  45,  45,   0,   0, -45,   0,   0,  100  }, // leg3,2 bk; leg1 fw
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,   0,   0, -20, -20,  45,   0,   0,  100  }, // leg3,2 up; leg3 fw
  {    0, -45,   0,   0,   0,   0,  45,   0,  100  }, // leg1,4 bk
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg3,2 dn
  {    0,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg4 up
  {    0,   0,   0,   0,   0,   0, -45, -20,  100  }, // leg4 fw dn
};

// Move Right
int servoPrg05step = 11;
const int servoPrg05 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {    0,   0,   0, -20, -20, -45,   0,   0,  100  }, // leg2,3 up; leg3 fw
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {   20,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg4,1 up
  {    0,   0, -45,   0,   0,  45,  45,   0,  100  }, // leg2,3 bk; leg4 fw
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg4,1 dn
  {    0,   0,  45, -20, -20,   0,   0,   0,  100  }, // leg2,3 up; leg2 fw
  {    0,  45,   0,   0,   0,   0, -45,   0,  100  }, // leg4,1 bk
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {   20,   0,   0,   0,   0,   0,   0,   0,  100  }, // leg1 up
  {  -20, -45,   0,   0,   0,   0,   0,   0,  100  }, // leg1 fw dn
};


// Turn left
int servoPrg06step = 8;
const int servoPrg06 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {   20,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg1,4 up
  {    0,  45,   0,   0,   0,   0,  45,   0,  100  }, // leg1,4 turn
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,   0,   0, -20, -20,   0,   0,   0,  100  }, // leg2,3 up
  {    0,   0,  45,   0,   0,  45,   0,   0,  100  }, // leg2,3 turn
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {    0, -45, -45,   0,   0, -45, -45,   0,  100  }, // leg1,2,3,4 turn
};

// Turn right
int servoPrg07step = 8;
const int servoPrg07 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {    0,   0,   0, -20, -20,   0,   0,   0,  100  }, // leg2,3 up
  {    0,   0, -45,   0,   0, -45,   0,   0,  100  }, // leg2,3 turn
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {   20,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg1,4 up
  {    0, -45,   0,   0,   0,   0, -45,   0,  100  }, // leg1,4 turn
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,  45,  45,   0,   0,  45,  45,   0,  100  }, // leg1,2,3,4 turn
};

// Lie
int servoPrg08step = 1;
const int servoPrg08 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {  110,  90,  90,  70,  70,  90,  90, 110,  500  }, // leg1,4 up
};

// Say Hi
int servoPrg09step = 4;
const int servoPrg09 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {  120,  90,  90, 110,  60,  90,  90,  70,  200  }, // leg1, 3 down
  {  -50,   0,   0,   0,  50,   0,   0,   0,  200  }, // standby
  {   50,   0,   0,   0, -50,   0,   0,   0,  200  }, // leg1, 3 down
  {  -50,   0,   0,   0,  50,   0,   0,   0,  200  }, // standby
};

// Fighting
int servoPrg10step = 11;
const int servoPrg10 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {  120,  90,  90, 110,  60,  90,  90,  70,  200  }, // leg1, 2 down
  {    0, -20, -20,   0,   0, -20, -20,   0,  200  }, // body turn left
  {    0,  40,  40,   0,   0,  40,  40,   0,  200  }, // body turn right
  {    0, -40, -40,   0,   0, -40, -40,   0,  200  }, // body turn left
  {    0,  40,  40,   0,   0,  40,  40,   0,  200  }, // body turn right
  {  -50, -20, -20, -40,  50, -20, -20,  40,  200  }, // leg1, 2 up ; leg3, 4 down
  {    0, -20, -20,   0,   0, -20, -20,   0,  200  }, // body turn left
  {    0,  40,  40,   0,   0,  40,  40,   0,  200  }, // body turn right
  {    0, -40, -40,   0,   0, -40, -40,   0,  200  }, // body turn left
  {    0,  40,  40,   0,   0,  40,  40,   0,  200  }, // body turn right
  {    0, -20, -20,   0,   0, -20, -20,   0,  200  }, // leg1, 2 up ; leg3, 4 down
};

// Push up
int servoPrg11step = 11;
const int servoPrg11 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  300  }, // start
  {   30,   0,   0, -30, -30,   0,   0,  30,  400  }, // down
  {  -30,   0,   0,  30,  30,   0,   0, -30,  500  }, // up
  {   30,   0,   0, -30, -30,   0,   0,  30,  600  }, // down
  {  -30,   0,   0,  30,  30,   0,   0, -30,  700  }, // up
  {   30,   0,   0, -30, -30,   0,   0,  30,  1300 }, // down
  {  -30,   0,   0,  30,  30,   0,   0, -30,  1800 }, // up
  {   65,   0,   0, -65, -65,   0,   0,  65,  200  }, // fast down
  {  -65,   0,   0,   0,  15,   0,   0,   0,  500  }, // leg1 up
  {    0,   0,   0,   0,  50,   0,   0,   0,  500  }, // leg2 up
  {    0,   0,   0,  65,   0,   0,   0, -65,  500  }, // leg3, leg4 up
};

// Sleep
int servoPrg12step = 3;
const int servoPrg12 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   35,  90,  90, 145, 145,  90,  90,  35,  400  }, // leg1,4 dn
  {    0, -45,  45,   0,   0,  45, -45,   0,  400  }, // protect myself
  {   80,   0,  0, -90,-90,   0,   0, +80,  1000  }, // protect myself
};

// Dancing 1
int servoPrg13step = 10;
const int servoPrg13 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   90,  90,  90,  90,  90,  90,  90,  90,  300  }, // leg1,2,3,4 up
  {  -40,   0,   0,   0,   0,   0,   0,   0,  300  }, // leg1 dn
  {   40,   0,   0,  40,   0,   0,   0,   0,  300  }, // leg1 up; leg2 dn
  {    0,   0,   0, -40,   0,   0,   0, -40,  300  }, // leg2 up; leg4 dn
  {    0,   0,   0,   0,  40,   0,   0,  40,  300  }, // leg4 up; leg3 dn
  {  -40,   0,   0,   0, -40,   0,   0,   0,  300  }, // leg3 up; leg1 dn
  {   40,   0,   0,  40,   0,   0,   0,   0,  300  }, // leg1 up; leg2 dn
  {    0,   0,   0, -40,   0,   0,   0, -40,  300  }, // leg2 up; leg4 dn
  {    0,   0,   0,   0,  40,   0,   0,  40,  300  }, // leg4 up; leg3 dn
  {    0,   0,   0,   0, -40,   0,   0,   0,  300  }, // leg3 up
};

// Dancing 2
int servoPrg14step = 9;
const int servoPrg14 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  45, 135, 110, 110, 135,  45,  70,  300  }, // leg1,2,3,4 two sides
  {   45,   0,   0, -45,   0,   0,   0,   0,  300  }, // leg1,2 up
  {  -45,   0,   0,  45, -45,   0,   0,  45,  300  }, // leg1,2 dn; leg3,4 up
  {   45,   0,   0, -45,  45,   0,   0, -45,  300  }, // leg3,4 dn; leg1,2 up
  {  -45,   0,   0,  45, -45,   0,   0,  45,  300  }, // leg1,2 dn; leg3,4 up
  {   45,   0,   0, -45,  45,   0,   0, -45,  300  }, // leg3,4 dn; leg1,2 up
  {  -45,   0,   0,  45, -45,   0,   0,  45,  300  }, // leg1,2 dn; leg3,4 up
  {   45,   0,   0, -45,  45,   0,   0, -45,  300  }, // leg3,4 dn; leg1,2 up
  {  -40,   0,   0,  40,   0,   0,   0,   0,  300  }, // leg1,2 dn
};

// Dancing 3
int servoPrg15step = 10;
const int servoPrg15 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  45,  45, 110, 110, 135, 135,  70,  300  }, // leg1,2,3,4 bk
  {   40,   0,   0, -50, -40,   0,   0,   0,  300  }, // leg1,2,3 up
  {  -40,   0,   0,  50,  40,   0,   0,   0,  300  }, // leg1,2,3 dn
  {   40,   0,   0,   0, -40,   0,   0,  50,  300  }, // leg1,3,4 up
  {  -40,   0,   0,   0,  40,   0,   0, -50,  300  }, // leg1,3,4 dn
  {   40,   0,   0, -50, -40,   0,   0,   0,  300  }, // leg1,2,3 up
  {  -40,   0,   0,  50,  40,   0,   0,   0,  300  }, // leg1,2,3 dn
  {   40,   0,   0,   0, -40,   0,   0,  50,  300  }, // leg1,3,4 up
  {  -40,   0,   0,   0,  40,   0,   0, -50,  300  }, // leg1,3,4 dn
  {    0,  45,  45,   0,   0, -45, -45,   0,  300  }, // standby
};

// up
int servoPrg16step = 1;
const int servoPrg16 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   60,  90,  90, 120, 120,  90,  90,  60,  100  }, // standby
};
//Get down
int servoPrg17step = 1;
const int servoPrg17 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   110,  90,  90,   60,   60,  90,  90,  110,  300 },
//  {   +20,   0,   0,  -20,  -20,   0,   0,  +20,  300 },
//  {     0,   0,   0,    0,    0,   0,   0,    0,  1000 },
};

//holle
int servoPrg18step = 9;
const int servoPrg18 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   0,  0,     0,   0,   0,  -60,  0,  0,  200  },
  {   +150,  +50,  0, 0, 0,  0,  0,  0,  300  },
  {   -70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   +70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   -70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   +70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   -70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   +70,  0,  0, 0, 0,  0,  0,  0,  300  },
//  {   +20,   0,   0,  -20,  -20,   0,   0,  +20,  300 },
//  {     0,   0,   0,    0,    0,   0,   0,    0,  1000 },
};

//Stand up
int servoPrg19step = 1;
const int servoPrg19 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   0,  90,  90, 180, 180,  90,  90,  0,  100 },
};

//

int servoPrg20step = 9;
const int servoPrg20 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   0,  +60,  0,    0,   0, -60,  0,  0,  200  },
  {   +150, 0,  0,    0,-150,  0,   0,  0,  300  },
  {   -70,  0,  0,    0,   0,  0,   0,  0,  300  },
  {   +70,  0,  0,    0, +70,  0,   0,  0,  300  },
  {   -70,  0,  0,    0, -70,  0,   0,  0,  300  },
  {   +70,  0,  0,    0, +70,  0,   0,  0,  300  },
  {   -70,  0,  0,    0, -70,  0,   0,  0,  300  },
  {   +70,  0,  0,    0, +70,  0,   0,  0,  300  },
}; 

  
void setup()
{
  Serial.begin(9600); // 初始化串行通信
  BT.begin(9600);

  pwm.begin();
  pwm.setPWMFreq(60); // 设置PWM频率为50Hz

  for(int i=0;i<8;i++)
  {
    setPosition(i, 90);
  
  }
}

void loop()
{
  if(BT.available()>0){
    char cmd = BT.read();
  
  switch (cmd) {
    case 'a':
      runServoPrgV(servoPrg16, servoPrg16step); // up
      cmd = 15;
      break;
    case 'b':
      runServoPrgV(servoPrg17, servoPrg17step); // Get down
      cmd = 15;
      break;
    case 'c':
      runServoPrgV(servoPrg10, servoPrg10step); // fighting
      break;
    case 'd':
      runServoPrgV(servoPrg18, servoPrg18step); // sayH
      cmd = 0;
      break;
    case 'e':
      runServoPrgV(servoPrg12, servoPrg12step); // sleep
      cmd = 15;
      break;
    case 'f':
      runServoPrgV(servoPrg19, servoPrg19step); // pushUp
      cmd = 15;
      break;
    case 'g':
      runServoPrgV(servoPrg20, servoPrg20step); // 
      break;
    case 'h':
      runServoPrgV(servoPrg13, servoPrg13step); // dancing1
      break;
    case 'i':
      runServoPrgV(servoPrg14, servoPrg14step); // dancing2
      break;

    case 'j':
        runServoPrgV(servoPrg00, servoPrg00step); // zero
        break;
    case 'k':
      runServoPrgV(servoPrg06, servoPrg06step); // turnLeft
      break;
    case 'l':
      runServoPrgV(servoPrg02, servoPrg02step); // forward
      break;
    case 'm':
      runServoPrgV(servoPrg07, servoPrg07step); // turnRight
       break;
    case 'n':
     runServoPrgV(servoPrg04, servoPrg04step); // moveLeft
      break;
    case 'o':
      runServoPrgV(servoPrg03, servoPrg03step); // backward
       break;
    case 'p':
     runServoPrgV(servoPrg05, servoPrg05step); // moveRight
       break;
}
}
}

void setPWM(int channel, int angle) {
  // 将角度转换为 PWM 脉冲宽度
  int pulse =  map(angle, 0, 180, MinPluse, MaxPluse); // 150-600 对应 0-180 度
  pwm.setPWM(channel, 0, pulse); // 设置通道的 PWM 脉冲宽度
}

bool setPosition(int index, int position)
{
  servo[index].position   = position;
  return true;
}

void run(int servoPrg[][9],int step){
 for(int i=0;i<8;i++)
 {
  int data = pgm_read_word(&servoPrg[0][i]);
 }
}

void runServoPrgV(int servoPrg[][9], int step) {
  for (int i = 0; i < step; i++) { // Loop for step

    int totalTime =pgm_read_word(&servoPrg[i][9 - 1]); // Total time of this step

    // Get servo start position
    for (int s = 0; s < 8; s++) {
      servoPos[s] = servo[s].position- servoCal[s];
    }

    for (int p = 0; p < 8; p++) { // Loop for servo
      if (i == 0) {
        servoPrevPrg[p] = pgm_read_word(&servoPrg[i][p]);
      } else {
        servoPrevPrg[p] = servoPrevPrg[p] + pgm_read_word(&servoPrg[i][p]);
      }
    }

    for (int j = 0; j < totalTime / servoPrgPeriod; j++) { // Loop for time section
      for (int k = 0; k < 8; k++) { // Loop for servo
        int r = map(j, 0, totalTime / servoPrgPeriod, servoPos[k], servoPrevPrg[k])+- servoCal[k];
        setPWM(servoIndex[k], r );
        
        servo[k].position=r;
      }
      delay(servoPrgPeriod);
    }
      
    }
}